#include <msp430x26x.h>
#include <stdio.h>
#include "Communication/UART_HANDLER.h"
#include "Hardware/PortDef.h"
#include "Sensors/Accel.h"
#include "Sensors/Mag.h"
#include "Sensors/Gyro.h"
#include "Radio/Radio.h"
#include "Controllers/PID.h"
#include "Gumstix/Gumstix.h"

/******************************************************
 * VERSION NUMBER - manually update this as substantive changes are made
 ******************************************************/
const char *VER_NUM = "3.0";   // this is the version number
/******************************************************/

/******************************************************
 * TYPES AND CONSTANT DEFINES
 ******************************************************/
 // 40ms sample period (also motor update period)
 #define SAMPLE_PERIOD 2
 // 240ms = 0.24s report period (4 times a second)
 #define REPORT_PERIOD 6
 #define START_CONVERSION ADC12SC
 // Servo control mid range value
 #define SERVO_MID 3333
 // Use the following defines to enable terminal debugging and gumstix data
 //  sending respectively.  Use with care as it will possibly affect the ability
 //  of the system to work in real time
// #define DEBUG
// #define GUMSTIX
 /*****************************************************/
 
 /******************************************************
 * MACROS
 ******************************************************/
 // Convert a float to fixed point
 #define FIX(num,frac) (num >= 0 ? ((long)((((long)num) << 8) | ((long)frac))) : (-((long)((((long)(-num)) << 8) | ((long)frac)))))
 // Truncates a fixed point number to its nearest whole number
 #define UNFIX(fix) ((long)(((long)fix) >> 8))
 /*****************************************************/

/******************************************************
 * INTERRUPT DECLARES - declare ISRs and link their handlers
 ******************************************************/
 __interrupt void TAOverflow(void);
 __interrupt void ADCDataReady(void);
/******************************************************/

/******************************************************
Variable Declarations, initializations 
******************************************************/
volatile unsigned char sampleCount;
volatile unsigned int gyroXSample, gyroYSample, gyroXYRefSample;
int gyroZZero;
int gyroZSample, gyroZTemp;
volatile unsigned int altSonar, leftSonar, rightSonar;
volatile unsigned int voltageLevel;
unsigned int accelX, accelY, accelZ;
int magX, magY, magZ;
int magDirection;
int desiredHeading;
int headingError;
long throttle;
long centerThrottle;
long raw_yaw;
long raw_pitch;
long raw_roll;
long motor1_yaw;
long motor2_yaw;

// Variables used to do motor control (duty cycles from radio)
long duty1, duty2, duty3, duty4;

// Controllers
char initControl;
int startup;
SPid YawControl;
SPid AltControl;
unsigned int lastSonar;
unsigned char sonarCount;
long error;
char yawmanualflag;

// Altitude command variables
long goalAltitude;
long rampState;

// Sonar filter
extern FIR_filter(int);
int input_delay0;
int input_delay1;
int input_delay2;
int input_delay3;
int input_delay4;
int input_delay5;
int input_delay6;
int input_delay7;
int input_delay8;
int input_delay9;
int input_delay10;
int output=0;

// Kill switch
char kill;

#ifdef DEBUG
	unsigned char reportCount;
	
	// UART string buffer
	char sensorReadStr[60];
#endif

#ifdef GUMSTIX
	SensorInfo sensorReadings;
	GumstixSyncState gumstixState;
#endif

/*****************************************************/

/*****************************************************
Function declarations
*****************************************************/
void ServMotCtl(void);
void InitSys(void);

/****************************************************/

/********************************************************
main - the main scheduling and code loop
*********************************************************/
void main(void)
{
	InitSys();
	
	// Initialize the SPI sensors needed
	//AccelInit();
	MagInit();
	GyroInit();
	
	while(1) // main polling loop 
	{
		S_parseRXCmd(); // Look for a command (this isnt really used at the moment, but it could be)
		
		if (!sampleCount)
		{
			
#ifdef DEBUG
			// Toggle timing pin (unused TX to Gumstix)
			//P3OUT |= UART_TxToGumstix;
#endif

			// Reinitialize the sample counter
			sampleCount = SAMPLE_PERIOD;
			
			// Get the sensor values needed
			//accelX = AccelGetX();
			//accelY = AccelGetY();
			//accelZ = AccelGetZ();
			magX = MagGetX();
			magY = MagGetY();
			//magZ = MagGetZ();
			magDirection = MagGetDirection(magX, magY);
			gyroZSample = GyroGetZ();
			//gyroZTemp = GyroGetTemp();
			
#ifdef GUMSTIX
				// Fill the gumstix packet with sensor values and motor/servo stuff
				sensorReadings.voltageLevel = voltageLevel;
				sensorReadings.magx = magX;
				sensorReadings.magy = magY;
				sensorReadings.magz = magZ;
				sensorReadings.gyrox = gyroXSample;
				sensorReadings.gyroy = gyroYSample;
				sensorReadings.gyroz = gyroZSample;
				sensorReadings.gyroxyref = gyroXYRefSample;
				sensorReadings.gyroztemp = gyroZTemp;
				sensorReadings.accelx = accelX;
				sensorReadings.accely = accelY;
				sensorReadings.accelz = accelZ;
				sensorReadings.altsonar = altSonar;
				sensorReadings.motor1 = motor1_yaw;
				sensorReadings.motor2 = motor2_yaw;
				sensorReadings.servo1 = raw_roll;
				sensorReadings.servo2 = raw_pitch;
				
				// Synchronization routine for the gumstix (uncomment debug UART stuff to debug)
				switch (gumstixState)
				{
					case GUMSTIX_STATE_INIT:
						if (GumstixIsReady())
						{
							gumstixState = GUMSTIX_STATE_READY;
							//sprintf(sensorReadStr, "Gumstix is Ready\n\r");
							//tx_slave_string(sensorReadStr);
							GumstixSendSync();
						}
						break;
					case GUMSTIX_STATE_READY:
						if (GumstixRxSync())
						{
							gumstixState = GUMSTIX_STATE_SYNCED;
							//sprintf(sensorReadStr, "Gumstix is Syncronized\n\r");
							//tx_slave_string(sensorReadStr);
						}
						break;
					case GUMSTIX_STATE_SYNCED:
						// This is where we send the sensor reading to the Gumstix
						GumstixTXInfo(&sensorReadings);
						break;
				}
#endif
			
			// Initialize yaw gyro zero value (initial values can be innacurate)
			if (initControl != 0)
			{
				initControl--;
				
				// We just wait to take the initial gyro and heading for a bunch of samples
				//  so if the sensors are screwy at startup, they can settle (this was seen
				//  in practice).  Remember to update the last sonar because we are doing
				//  differences to get derivative.
				if (initControl == 0)
				{
					gyroZZero = gyroZSample;
					desiredHeading = magDirection;
				}
				lastSonar = altSonar;
			}
			else
			{
				// Control the helicopter if we are not initializing
				ServMotCtl();
				
				// Get the heading error
				headingError = desiredHeading - magDirection;
				
				// Correct the heading error so its in the range of [-180,180]
				if (headingError > 180)
				{
					headingError -= 360;
				}
				else if (headingError < -180)
				{
					headingError += 360;
				}
				
				// Update the controllers (assumes raw_yaw in fixed point) (limit to [-80,80])
				raw_yaw = UNFIX(PIDUpdate(&YawControl, FIX(headingError,0), FIX(((int)gyroZSample - (int)gyroZZero),0)));
				raw_yaw = raw_yaw > 0 ? (raw_yaw > 80 ? 80 : raw_yaw) : (raw_yaw < -80 ? -80 : raw_yaw);
				raw_yaw = raw_yaw - (throttle / 14);	// Add a mixing factor, helped balance the motors in practice
				
				// Filter the sonar (this can be used but will add a delay and screw up the current control system)
//				output = 0;
//				input_delay0 = altSonar;
//				FIR_filter(input_delay0);
//				altSonar = output;

				// Filter the sonar (actually just remove outliers - assumes two consecutive readings must differ by less than 200)
				if (altSonar > (lastSonar + 200))
				{
					altSonar = lastSonar;
				}
				else if ((lastSonar > 200) && altSonar < (lastSonar - 200))
				{
					altSonar = lastSonar;
				}
				
				// Throttle control initialization routine
				if (startup < 370)
				{
					throttle = startup;
					startup += 2;
				}
				else
				{
					// Calculate the error
					error = rampState - FIX((int)altSonar,0);
					
					// Throttle control
					throttle = UNFIX(PIDUpdate(&AltControl, error, FIX(((int)altSonar - (int)lastSonar),0)));
					throttle = throttle > 0 ? (throttle > 129 ? 129 : throttle) : (throttle < -129 ? -129 : throttle);
					throttle = throttle + 370;
					
					// Update the command (if we are ramping up to a command, continue to do so
					if (rampState > goalAltitude)
					{
						rampState -= FIX(2,0x00);
					}
					else if (rampState < goalAltitude)
					{
						rampState += FIX(2,0x00);
					}
				}
				
				// Update sonar
				lastSonar = altSonar;
				
				// If the kill switch is activated, just keep controllers, throttle, startup and ramp at bay - zeros
				if (kill == 1)
				{
					AltControl.iState = 0;
					YawControl.iState = 0;
					throttle = 0;
					startup = 0;
					rampState = FIX(0,0x00);
				}
			}
			
#ifdef DEBUG
			// Report the results 4x a second
			reportCount--;
			if (reportCount == 0)
			{
				reportCount = REPORT_PERIOD;
				
				//sprintf(sensorReadStr, "%d, %d, %d\n\r", gyroXSample, gyroYSample, gyroXYRefSample);
				//sprintf(sensorReadStr, "%d, %d, %d\n\r", accelX, accelY, accelZ);
				//sprintf(sensorReadStr, "%u\n\r", altSonar);
				//sprintf(sensorReadStr, "%d\n\r", accelZ);
				//sprintf(sensorReadStr, "%d\t%d\t%d\n\r", magY, magZ, magDirection);
				//sprintf(sensorReadStr, "%d\t%d\n\r", gyroZSample, gyroZTemp);
				//sprintf(sensorReadStr, "D: %lu\t%lu\t%lu\t%lu\n\r", duty1, duty2, duty3, duty4);
				//sprintf(sensorReadStr, "D: %lu\n\r", duty4);
				//sprintf(sensorReadStr, "%u\t%u\t%u\n\r", RadioCh1.servoIn1Course, RadioCh1.servoIn2Course, RadioCh1.servoIn3Course);
				//sprintf(sensorReadStr,"%ld\n\r", throttle);
				//sprintf(sensorReadStr, "D: %lu\t%lu\n\r", duty3, per3);
				//sprintf(sensorReadStr, "D: %lu\t%lu\t%d\n\r", motor1_yaw, motor2_yaw, headingError);
				//sprintf(sensorReadStr, "D: %ld\t%d\t%d\n\r", raw_yaw, magDirection, desiredHeading);
				//sprintf(sensorReadStr, "D: %ld\t%u\t%u\n\r", delta_roll, gyroYZero, gyroYSample);
				//sprintf(sensorReadStr, "D: %ld\n\r", AltControl.dState);
				//sprintf(sensorReadStr, "D: %ld\t%d\n\r", raw_yaw, headingError);
				//sprintf(sensorReadStr, "%d\t%d\t%d\t%d\t%d\n\r", magDirection, magX, magY,gyroZSample, gyroZZero);
				//sprintf(sensorReadStr, "%d\t%d\t%d\n\r", magX, magY, magDirection);
				sprintf(sensorReadStr, "%d\t%u\t%d\t%d\n\r", gyroZSample, altSonar, magDirection,(int)sonarCount);
				tx_slave_string(sensorReadStr);
			}
#endif
			
			ADC12CTL0 |= START_CONVERSION;  // Enable another round of conversions
			
#ifdef DEBUG
			// Toggle timing pin (unused TX to Gumstix)
			//P3OUT &= ~UART_TxToGumstix;
#endif
		}
	}
}
/********************************************************/

/******************************************************
 * InitSys - initialize and configure system after reset
 ******************************************************/
void InitSys(void)
{
	// *********************** CLOCKS ****************************************
 	//XT1 has a 32.768kHz crystal attached
 	//XT2 has a 16MHz crystal attached
 	int i;
 	WDTCTL = WDTPW + WDTHOLD;   // Stop WDT
 	_disable_interrupts();		// DINT
 	_bic_SR_register(OSCOFF);	// Turn on oscillators
 	BCSCTL1 = 0x00;				// XT2 ON, LFXT1 = low freq, ACLK div = 1
	BCSCTL3 = (XT2S_2+LFXT1S_0+XCAP_3); //XT2 uses 16MHz crystal, XT1 in low freq op, XT1 uses 12.5pf cap, 
	do
	{
		IFG1 &= ~(OFIFG);				// Clear OFIFG
		for(i = 0; i<255;i++) _nop();	// Delay
	}
	while(IFG1&OFIFG);			// Loop while OFIFG bit in IFG1 = 1
	BCSCTL2 = (SELM_2+DIVM_0+SELS+DIVS_1);		// MCLK=XT2, div=1, SMCLK=XT2, div = 2
	//so, at this point the ACLK=32768Hz, MCLK=16MHz, SMCLK=8MHz, DCO is off

	// *********************** PORTS *****************************************
	//setup outputs (low by default)
  	P1OUT = 0;
	P2OUT = (0+SPIEnblGyroZ+SPIEnblAccel+SPIEnblPres);									// SPI enables are active low, so deactivate by setting high
	P3OUT = (0+SPIEnblMag);
	P4OUT = 0;
	P5OUT = 0;
	P6OUT = 0;

	//  I/O configured as outputs by default to eliminate floating inputs.
  	P1DIR = 0xFF;
  	P2DIR = (0xFF & ~MagDatRdy & ~PresDatRdy); 	                            			// MagDatRdy, PresDatRdy are inputs
  	P3DIR = (0xFF & ~SPIMiSo);					                            			// MiSo is an input
  	P4DIR = (0xFF & ~Serv1PWMIn & ~Serv2PWMIn & ~Serv3PWMIn & ~Serv4PWMIn); 			// All P4.x outputs
  	P5DIR = 0xFF;   							                            			// All P5.x outputs
  	P6DIR = 0xFF;								                            			// All P6.x outputs
	
	// Set up pin special functions.
	//Board revision - replace commented ports
    P1SEL = (Mot1PWMOut+Mot2PWMOut);													// PWM outputs from Timer (Motors)
    P3SEL = (UART_TxToGumstix+UART_RxFromGumstix+UART_TxToGPS+UART_RxFromGPS);			// connect to UARTs
	P4SEL = (Serv1PWMOut+Serv2PWMOut+Serv1PWMIn+Serv2PWMIn+Serv3PWMIn+Serv4PWMIn);		// connect to timer module
	P6SEL = (Son1In+Son2In+Son3In+Son4In+GyroX+GyroY+GyroXYRef);						// connect to ADC
	
#ifdef DEBUG
	// Initialize the UART
	init_uart_tx_rx();
	
	tx_byte_to_slave(0x0D);
	tx_byte_to_slave(0x0A);
	tx_slave_string("IMUboard restart");
	tx_byte_to_slave(0x0D);
	tx_byte_to_slave(0x0A);
#endif
	
	// ************************ Timer A  ********
	TACCTL0 = 0;										//Don't enable interrupts (we don't want to interrupts this often)
	TACCTL1 = (OUTMOD_7);               				//CCR1 set mode (reset/set)
	TACCTL2 = (OUTMOD_7);               				//CCR2 set mode (reset/set)
	TACCR0 = 499; 	                    				//timer period = 250uS, which is 500-1 clock cycles (since 0 gets counted)
	TACCR1 = 0;											//Servo PWM signal (twice timer rate of old code)
	TACCR2 = 0;											//Servo PWM signal (twice timer rate of old code)
	TACTL = (TASSEL_2 + ID_2+ MC_1);    				//SMCLK, /4, up mode, interrupts enabled, compare mode
 
	// *************** Timer B *******************
	TBCCTL0 = (CCIE);									//Enable interrupt for task scheduling
	TBCCTL1 = (OUTMOD_7);                           	//CCR1 reset/set
	TBCCTL2 = (OUTMOD_7);                           	//CCR2 reset/set
	TBCCTL3 = (CCIS_0 + CM_3 + CAP + SCS + CCIE);   	//capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
	TBCCTL4 = (CCIS_0 + CM_3 + CAP + SCS + CCIE);   	//capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
	TBCCTL5 = (CCIS_0 + CM_3 + CAP + SCS + CCIE);   	//capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
	TBCCTL6 = (CCIS_0 + CM_3 + CAP + SCS + CCIE);   	//capture/compare input select, capture on rising and falling edge, capture mode, capture/compare interrupt enabled
	TBCCR0 = 39999;                                   	//PWM period = 20ms us, which is 40000-1 clock cycles
	TBCCR1 = 0;											//Servo PWM signal
	TBCCR2 = 0;											//Servo PWM signal
	TBCTL = (TBSSEL_2 + ID_2+ MC_1);    				//SMCLK, /4, up mode
	
	//**************** ADC ***********************
	// This is all ADC sensors (series of auto conversions)
//	ADC12CTL0 = (MSC + REFON + ADC12ON + REF2_5V +      //auto continuous conversions, reference generator on, ADC12 on, 2.5V ref
//	             SHT0_15);								//Make conversions sample/hold for 1024 counts (at 5MHz/8) just in case
//	ADC12CTL1 = (CSTARTADD_0 + SHS_0 + SHP +            //Start saving at ADC12MEM0 register, sample and hold init by ADC12SC, ?SAMPCON source?
//	             /*ISSH*/ + ADC12DIV_7 + ADC12SSEL_0 +  //Input not inverted, no clock division, ADC12OSC oscillator
//	             CONSEQ_1);                             //Sequence of channels, no busy signal used
//	ADC12MCTL4 = (EOS + SREF_1 + INCH_7);				//Last conversion in sequence (seventh conversion), ref: VR+ = Vref and VR- = AVSS, (channel A7 - Voltage Monitor)
//	ADC12MCTL3 = (SREF_1 + INCH_3);						//ref: VR+ = Vref and VR- = AVSS, (channel A3 - Altitude Sonar (4))
//	ADC12MCTL4 = (SREF_1 + INCH_1);						//ref: VR+ = Vref and VR- = AVSS, (channel A? - Left Sonar (?))
//	ADC12MCTL3 = (SREF_1 + INCH_2);               		//ref: VR+ = Vref and VR- = AVSS, (channel A?- Right Sonar (?))
//	ADC12MCTL2 = (SREF_1 + INCH_6);                     //ref: VR+ = Vref and VR- = AVSS, (channel A6 - GYROXYRef)
//	ADC12MCTL1 = (SREF_1 + INCH_5);                     //ref: VR+ = Vref and VR- = AVSS, (channel A5 - GYROY)
//	ADC12MCTL0 = (SREF_1 + INCH_4);         			//ref: VR+ = Vref and VR- = AVSS, (channel A4 - GYROX)
//	ADC12IE = 0x10;                                     //turn on interrupt for register 4 (last conversion in sequence)
//	ADC12CTL0 |= ENC;                      				//enable conversion
//	ADC12CTL0 |= START_CONVERSION;						//start conversion
	// Just altitude sonar sampling
	ADC12CTL0 = (REFON + ADC12ON + REF2_5V + SHT0_15);
	ADC12CTL1 = (CSTARTADD_0 + SHS_0 + SHP + ADC12DIV_7 + ADC12SSEL_1);
	ADC12MCTL0 = (SREF_1 + INCH_3);
	ADC12IE = 0x01;
	ADC12CTL0 |= ENC;
	ADC12CTL0 |= START_CONVERSION;
	
	// *************** Initial Values ************
	sampleCount = SAMPLE_PERIOD;

#ifdef DEBUG
	reportCount = REPORT_PERIOD;
#endif

#ifdef GUMSTIX
	// Setup the sensor info structure
	sensorReadings.SOP = 0xDEAD;
	sensorReadings.EOP = 0xBEEF;
	GumstixInit();
	gumstixState = GUMSTIX_STATE_INIT;
#endif

	// Initialize the duty cycle values of the radio, throttle, and radio module
	duty1 = duty2 = duty3 = duty4 = 0;
	throttle = 0;
	RadioInit();
	
	// *************** Setup the PID controllers *
	YawControl.dState = 0;
	YawControl.iState = 0;
	YawControl.iMax = FIX(320,0x00);
	YawControl.iMin = FIX(-320,0x00);
	YawControl.iGain = FIX(0,0x08); 	// I gain of 1/32
	YawControl.pGain = FIX(0,0x80); 	// P gain of 1/2
	YawControl.dGain = FIX(0,0x40); 	// D gain of 1/4
	
	AltControl.dState = 0;
	AltControl.iState = 0;
	AltControl.iMax = FIX(320,0x00);	// I max of 10 throttle
	AltControl.iMin = FIX(-320,0x00);	// I min of -10 throttle
	AltControl.iGain = FIX(0,0x08);		// I gain of 1/32
	AltControl.pGain = FIX(0,0x40); 	// P gain of 1/4
	AltControl.dGain = FIX(3,0x00); 	// D gain of 3
	
	// Initialize startup stuff
	initControl = 20;
	startup = 0;
	desiredHeading = 0;
	gyroZZero = 0;
	kill = 1;
	yawmanualflag = 0;
	
	// Initialize sonar filter values
	input_delay0 = 0;
	input_delay1 = 0;
	input_delay2 = 0;
	input_delay3 = 0;
	input_delay4 = 0;
	input_delay5 = 0;
	input_delay6 = 0;
	input_delay7 = 0;
	input_delay8 = 0;
	input_delay9 = 0;
	input_delay10 = 0;
	sonarCount = 0;
	
	// Trajectory stuff
	//goalAltitude = FIX(400,0x00);
	goalAltitude = FIX(100,0x00);	// Start the goal altitude at 100 just cause, sonar cant read below 6in, might get screwy if this was zero
	rampState = FIX(0,0x00);
						
	_enable_interrupts();	//this is the last thing that the Init Routine should do, since we don't want ISR activity before everything is initialized  		
}
/********************************************************/

/*********************************************************
ServMotCtl() - uses global state variables to command the servo and motor controls
performs mixing from thrust and yaw to motor commands
*********************************************************/
void ServMotCtl(void)
{
	_disable_interrupts();	// Want to make this automic
	
	// Read the current duty cycles
	duty1 = RadioCh1.dutyCycle;
	duty2 = RadioCh2.dutyCycle;
	duty3 = RadioCh3.dutyCycle;
	duty4 = RadioCh4.dutyCycle;
	
	_enable_interrupts();  // Reenable interrupts
	
	// Convert servos to 39999 count PWM duty cycles in range [2222,4444] out of 39999
	// Low = 1ms (2000 counts), High = 2ms (4000 counts), Period = 20ms
	raw_pitch = duty2;
	if (raw_pitch < 2000) raw_pitch = 2000;
	else if (raw_pitch > 3999) raw_pitch = 3999;
	raw_roll = duty1;
	if (raw_roll < 2000) raw_roll = 2000;
	else if (raw_roll > 3999) raw_roll = 3999;
	
//	// Kill switch
//	if (duty3 > 3000)
//	{
//		kill = 0;
//	}
//	else
//	{
//		kill = 1;
//	}

	// Adjustable height from throttle stick (above 2500 on throttle radio input)
	if (duty3 >= 2400)
	{
		kill = 0;
		
		if (duty3 < 2500)
		{
			goalAltitude = FIX(100,0x00);
		}
		else
		{
			// Use the throttle in (duty3) as desired altitude setting
			goalAltitude = 100 + ((duty3 - 2500) >> 1) << 1;
			goalAltitude = FIX(goalAltitude,0x00);
		}
	}
	else
	{
		kill = 1;
	}
	
	// Preprocess the motor servo values (get them into full scale range [0,499])
	// Uncomment this for manual throttle (make sure to remove control)
//	throttle = duty3 - 2100;  // Slightly hacked because low throttle is moving
//	if (throttle > 1999) throttle = 1999;
//	else if (throttle < 0) throttle = 0;
	duty4 = duty4 - 2017;				// Slightly hacked based on flight and debugging
	if (duty4 > 2000) duty4 = 2000;
	else if (duty4 < 0) duty4 = 0;
	duty4 = (duty4/10) - 100;  // [-100,100]
	
	// Convert motors to 499 count PWM duty cycles (Uncomment for manual)
//	throttle = throttle >> 2;	// Throttle (divide by 4 to get in range)
	
	// Update the output servo positions (the commented stuff was used as a visual indicator the gumstix synching, dont uncomment unless Gumstix is attached)
//	if (gumstixState != GUMSTIX_STATE_SYNCED)
//	{
//		TBCCR1 = 4000;
//	}
//	else
//	{
		TBCCR1 = raw_roll;      //ROLL
//	}
	TBCCR2 = raw_pitch;     //PITCH
	
	// Control yaw, either from transmitter (if outside [-5,5]) of from control system
	if ((duty4 < -5) || (duty4 > 5))
	{
		raw_yaw = duty4;
		yawmanualflag = 1;
	}
	else if (yawmanualflag == 1)
	{
		YawControl.iState = 0;
		desiredHeading = magDirection;
		yawmanualflag = 0;
	}
	
	// Yaw adjustment [0,200] -> [-100,100] -> [-50,50] (split over both motors)
	// NOTE: Can use YAW trim adjustment to make sure yaw is split evenly (use duty4 if manual is desired)
	raw_yaw = (raw_yaw >> 1);
	
	// Do the mixing, essentially, we just add yaw to one motor and subtract from the other (appropriately)
	//  unless we are at the bounds of power (0 or 500) at which point we put the motor to the max or min
	//  and place the rest of the difference on the other motor.
	if (raw_yaw > throttle)
	{
		motor1_yaw = throttle;
		motor2_yaw = 0;
	}
	else if (-raw_yaw > throttle)
	{
		motor1_yaw = 0;
		motor2_yaw = throttle;
	}
	else if (throttle + raw_yaw > 499)
	{
		motor1_yaw =  499;
		motor2_yaw = (998 - (raw_yaw << 1) - throttle);
	}
	else if (throttle - raw_yaw > 499)
	{
		motor1_yaw = (998 + (raw_yaw << 1) - throttle);
		motor2_yaw = 499;
	}
	else
	{
		motor1_yaw = throttle + raw_yaw;
		motor2_yaw = throttle - raw_yaw;
	}
	
	// Output the motor PWMs
	TACCR1 = motor1_yaw;		// Top rotor
	TACCR2 = motor2_yaw;		// Bottom Rotor
}

//*****************************************************
//ISR: Sensor Reading
//Timer A interrupt, occurs every 20ms
//*****************************************************
#pragma vector=TIMERB0_VECTOR
__interrupt void TAOverflow(void)
{
	if (sampleCount > 0) sampleCount--;
}

//*****************************************************
//ISR: ADC data ready
//ADC data ready interrupt, called when conversion ready
//*****************************************************
#pragma vector=ADC12_VECTOR                                                                                                                                                    
 __interrupt void ADCDataReady(void)
{
	// Save the samples
	// When we want to use all of the ADC sensors, we uncomment this, remove the uncommented line
	//  and change the setup of the ADC appropriately, see that section
// 	gyroXSample = ADC12MEM0;
// 	gyroYSample = ADC12MEM1;
// 	gyroXYRefSample = ADC12MEM2;
// 	rightSonar = ADC12MEM3;
// 	leftSonar = ADC12MEM4;
// 	altSonar = ADC12MEM3;
// 	voltageLevel = ADC12MEM4;
	altSonar = ADC12MEM0;
}
